// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Tools [Debug].cpp
//  Author:    Andrew Aye (EMail: andrew.aye@gmail.com, Web: http://www.andrewaye.com) 
//  Version:   3.11
//
// ------------------------------------------------------------------------------------------------------------------------------ //
//
//  Copyright: © 2002-2008, Andrew Aye.  All Rights Reserved.
//
//  This software is free for non-commercial use. Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met: 
//    Redistributions of source code must retain this copyright notice, this list of conditions and the following disclaimers. 
//    Redistributions in binary form must reproduce this copyright notice, this list of conditions and the following
//      disclaimers in the documentation and other materials provided with the distribution. 
//
//  Neither the names of the copyright owner nor the names of its contributors may be used to endorse or promote products derived
//  from this software without specific prior written permission. 
//
//  The intellectual property rights of the algorithms used reside with Andrew Aye.  You may not use this software, in whole or
//  in part, in support of any commercial product without the express written consent of the author.
//
//  There is no warranty or other guarantee of fitness of this software for any purpose. It is provided solely "as is".
//
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //

#include "TgS COLLISION/TgS Collision - Tools [Debug].h"




namespace TGS { // START TGS ///////////////////////////////////////////////////////////////////////////////////////////////////////
namespace COL { // START COL ///////////////////////////////////////////////////////////////////////////////////////////////////////

// ============================================================================================================================== //

template <typename TYPE, int DIM>
TTgCOL_DEBUG<TYPE,DIM>::TTgCOL_DEBUG()
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Constructor()
{
    Reset();
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Reset()
{
    m_bfFlags.Reset();
    m_niDraw = 0;
    m_tvS0 = MATH::F_0<TYPE,DIM>();
    m_tvT0 = MATH::F_0<TYPE,DIM>();
    m_tvV0 = MATH::F_0<TYPE,DIM>();
    m_tvFF = MATH::F_MUL( T_(VECTOR,DIM)::UNIT_Y, TYPE(-32.0) );
    m_tvUFF = T_(VECTOR,DIM)::UNIT_Y;
    m_tyFF = TYPE(32.0);
    m_tyTime = TYPE(0.0);
    m_niProjectile = 0;
    m_niContact = 0;
    m_niTri = 0;
    m_uiTriMask = 0;
    MATH::F_CLI( &m_tmRF );

    for (TgINT iIdx = 0; iIdx < KTgMAX_DEBUG_POINT; ++iIdx)
    {
        MATH::F_Set_Max_Vector( m_atvPoint[iIdx] );
    };

    for (TgINT iIdx = 0; iIdx < KTgMAX_DEBUG_SEGMENT; ++iIdx)
    {
        MATH::F_Set_Max_Vector( m_atvSG[iIdx*2] );
        MATH::F_Set_Max_Vector( m_atvSG[iIdx*2 + 1] );
    };

    register T_(VECTOR,DIM)             tvP0;

    MATH::F_Set_Max_Vector( tvP0 );

    for (TgINT iIdx = 0; iIdx < KTgMAX_DEBUG_TRI; ++iIdx)
    {
        m_atvPT[iIdx].Set_PointDN( tvP0, tvP0, tvP0, tvP0 );
    };
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Execute()
{
    // Gather History Information
    // Debug Controller Processing
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_ReferenceFrame( const TTgMATRIX<TYPE,3,4> &tmTX )
{
    m_tmRF = tmTX;
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Reset_ReferenceFrame()
{
    MATH::F_CLI( &m_tmRF );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_PC( CR_(PARTICLE,DIM) tgPC0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_LN( CR_(LINE,DIM) tgLN0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_RY( CR_(RAY,DIM) tgR0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_SG( CR_(SEGMENT,DIM) tgG0, C_TgUINT32 uiCol )
{
    Draw_SG( tgG0.Query_Origin(), MATH::F_ADD( tgG0.Query_Origin(), tgG0.Query_DirN() ), uiCol );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_CI( CR_(CIRCLE,DIM) tgCI0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_DK( CR_(DISK,DIM) tgDK0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_EL( CR_(ELLIPSE,DIM) tgEL0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_PT( CR_(PTRI,DIM) tgPT0, C_TgUINT32 uiCol )
{
    register const T_(VECTOR,DIM)       tvP0 = MATH::F_ADD( tgPT0.Query_Point0(), MATH::F_MUL( m_tyPush, tgPT0.Query_Normal() ) );
    register const T_(VECTOR,DIM)       tvP1 = MATH::F_ADD( tgPT0.Query_Point1(), MATH::F_MUL( m_tyPush, tgPT0.Query_Normal() ) );
    register const T_(VECTOR,DIM)       tvP2 = MATH::F_ADD( tgPT0.Query_Point2(), MATH::F_MUL( m_tyPush, tgPT0.Query_Normal() ) );

    Draw_PT( tvP0,tvP1,tvP2, uiCol );

    register const T_(VECTOR,DIM)       tvP3 = MATH::F_MUL( MATH::F_ADD( tvP0, MATH::F_ADD( tvP1, tvP2 ) ), TYPE(1.0) / TYPE(3.0) );

    if (m_bfFlags.Query_Flag( ETgFLAG_DRAW_NORMAL ))
    {
        Draw_SG( tvP3, MATH::F_ADD( tvP3, MATH::F_MUL( TYPE(4.0), tgPT0.Query_Normal() ) ), uiCol );
    };
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_CT( CR_(CTRI,DIM) tgCT0, C_TgUINT32 uiCol )
{
    Draw_PT( tgCT0.Query_PT(), uiCol );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_RT( CR_(RECTANGLE,DIM) tgRT0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_PM( CR_(PARALLELOGRAM,DIM) tgPM0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_SP( CR_(SPHERE,DIM) tgSP0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_BX( CR_(BOX,DIM) tgBX0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_BA( CR_(BOXAA,DIM) tgBA0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_CP( CR_(CAPSULE,DIM) tgCP0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_CY( CR_(CYLINDER,DIM) tgCY0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_MH( CR_(MESH_AABB,DIM) tgMH0, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_SG( M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvS1, C_TgUINT32 uiCol )
{
//    RND->DRAWLINE( tvS0, tvS1, uiCol );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_PT( M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvS1, M_(VECTOR,DIM) tvS2, C_TgUINT32 uiCol )
{
    Draw_SG( tvS0, tvS1, uiCol );
    Draw_SG( tvS0, tvS2, uiCol );
    Draw_SG( tvS1, tvS2, uiCol );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_SP( M_(VECTOR,DIM) tvS0, const TYPE                          tyRadius, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_BA( M_(VECTOR,DIM) tvMin, M_(VECTOR,DIM) tvMax, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_CY( M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvS1, const TYPE                          tyRadius, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_CP( M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvS1, const TYPE                          tyRadius, C_TgUINT32 uiCol )
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_Contact_With_Text( CR_(CONTACT,DIM) F_Contact )
{
    Draw_Contact( F_Contact );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_Contact( CR_(CONTACT,DIM) tgContact )
{
    Draw_SP( tgContact.m_tvPos, TYPE(0.25), 0xFF808080 );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_Axis()
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Draw_Projectile()
{
    if (!m_bfFlags.Query_Flag( ETgFLAG_DRAW_PROJECTILE ) || (m_tyTime <= 0.0f))
    {
        return;
    }

    // TgRND_SET_STATE( ETgRND_STATE_ALPHA, TgTRUE );

    const TYPE                          tyLength = m_tyTime / TYPE(m_niProjectile);
    T_(VECTOR,DIM)                      tvPoint[2];

    tvPoint[0] = m_tvS0;

    for (TgINT iIdx = 1; iIdx < m_niProjectile; ++iIdx)
    {
        const TYPE                          tyT = tyLength*TYPE(iIdx);

        tvPoint[iIdx&1] = MATH::F_ADD( MATH::F_ADD( MATH::F_MUL( TYPE(0.5)*tyT*tyT, m_tvFF ), MATH::F_MUL( m_tvV0, tyT ) ), m_tvS0 );
        Draw_SG( tvPoint[0], tvPoint[1], (iIdx&1) ? 0xFFC00000 : 0xFF00C000 );
    };
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Render()
{
    // TgRND_SET_STATE( ETgRND_STATE_ALPHA, TgTRUE );
    // TgRND_SET_STATE( ETgRND_STATE_LIGHT, TgFALSE );
    // TgRND_SAVE_STATE( ETgRND_STATE_AMBIENT );
    // TgRND_SAVE_STATE( ETgRND_STATE_CULLMODE );
    // TgRND_SAVE_STATE( ETgRND_STATE_ZBUFFER );

    for (TgINT iIdx = 0; iIdx < KTgMAX_DEBUG_POINT; ++iIdx)
    {
        if (!MATH::F_Is_Max_Vector( m_atvPoint[iIdx] ))
        {
            Draw_SP( m_atvPoint[iIdx], 0.25F, 0xFFFFFFFF );
        };
    };

    for (TgINT iIdx = 0; iIdx < KTgMAX_DEBUG_SEGMENT; ++iIdx)
    {
        if (!MATH::F_Is_Max_Vector( m_atvSG[iIdx*2] ) && !MATH::F_Is_Max_Vector( m_atvSG[iIdx*2 + 1] ))
        {
            Draw_SG( m_atvSG[iIdx*2], m_atvSG[iIdx*2 + 1], 0xFFFFFFFF );
        };
    };

    for (TgINT iIdx = 0; iIdx < KTgMAX_DEBUG_TRI; iIdx += 2)
    {
        if (
            !MATH::F_Is_Max_Vector( m_atvPT[iIdx].Query_Point0() ) &&
            !MATH::F_Is_Max_Vector( m_atvPT[iIdx].Query_Point1() ) &&
            !MATH::F_Is_Max_Vector( m_atvPT[iIdx].Query_Point2() )
        ) {
            Draw_PT( m_atvPT[iIdx], 0xFFFFFFFF );
        };
    };

    if (Query_Draw_Projectile())
    {
        Draw_Projectile();
    };

    for (TgINT iIdx = 0; iIdx < m_niContact; ++iIdx)
    {
        Draw_Contact( m_atgContact[iIdx] );
    };

    for (TgINT iIdx = 0; iIdx < m_niTri; ++iIdx)
    {
        switch (m_aiID[iIdx]) {
            case 0:
            default:
                Draw_PT( m_atgPT[iIdx], 0xFFFFFFFF );
                break;
        };
    };

    Reset_Render();
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Reset_Render()
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Reset_Control()
{
    m_bfFlags.Set_Flag( ETgFLAG_STEP_COLLIDE, TgFALSE );
    m_bfFlags.Set_Flag( ETgFLAG_STEP_SIM, TgFALSE );
    m_bfFlags.Set_Flag( ETgFLAG_SKIP_SIM, TgFALSE );

    m_niContact = 0;
    m_niTri = 0;
    m_niDraw = 0;
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Reset_Input()
{

};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Can_Pause( C_TgBOOL bFlag )
{
    m_bfFlags.Set_Flag( ETgFLAG_CAN_PAUSE, bFlag );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Pause( C_TgBOOL bFlag )
{
    m_bfFlags.Set_Flag( ETgFLAG_PAUSE, bFlag );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Step_Collide( C_TgBOOL bFlag )
{
    m_bfFlags.Set_Flag( ETgFLAG_STEP_COLLIDE, bFlag );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Step_Sim( C_TgBOOL bFlag )
{
    m_bfFlags.Set_Flag( ETgFLAG_STEP_SIM, bFlag );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Skip_Sim( C_TgBOOL bFlag )
{
    m_bfFlags.Set_Flag( ETgFLAG_SKIP_SIM, bFlag );
};


template <typename TYPE, int DIM>
TgBOOL TTgCOL_DEBUG<TYPE,DIM>::Query_Can_Pause()
{
    return (m_bfFlags.Query_Flag( ETgFLAG_CAN_PAUSE ));
};


template <typename TYPE, int DIM>
TgBOOL TTgCOL_DEBUG<TYPE,DIM>::Query_Pause()
{
    return (m_bfFlags.Query_Flag( ETgFLAG_PAUSE ));
};


template <typename TYPE, int DIM>
TgBOOL TTgCOL_DEBUG<TYPE,DIM>::Query_Step_Collide()
{
    return (m_bfFlags.Query_Flag( ETgFLAG_STEP_COLLIDE ));
};


template <typename TYPE, int DIM>
TgBOOL TTgCOL_DEBUG<TYPE,DIM>::Query_Step_Sim()
{
    return (m_bfFlags.Query_Flag( ETgFLAG_STEP_SIM ));
};


template <typename TYPE, int DIM>
TgBOOL TTgCOL_DEBUG<TYPE,DIM>::Query_Skip_Sim()
{
    return (m_bfFlags.Query_Flag( ETgFLAG_SKIP_SIM ));
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Projectile( M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvT0, const TYPE tyTime )
{
    C_(VECTOR,DIM)                      tvDN = MATH::F_SUB( tvT0, tvS0 );
    const TYPE                          tyV = MATH::F_DOT( tvDN, m_tvUFF );
    TYPE                                tyU;
    C_(VECTOR,DIM)                      tvDH = MATH::F_NORM( &tyU, MATH::F_SUB( tvDN, MATH::F_MUL( tyV, m_tvUFF ) ) );
    const TYPE                          tySH = tyU / tyTime;
    const TYPE                          tySV = tyV / tyTime - TYPE(0.5)*m_tyFF*tyTime;

    m_tvS0 = tvS0;
    m_tvT0 = tvT0;
    m_tyTime = tyTime;

    m_tvV0 = MATH::F_ADD( MATH::F_MUL( tySH, tvDH ), MATH::F_MUL( tySV, m_tvUFF ) );

    m_bfFlags.Set_Flag( ETgFLAG_DRAW_PROJECTILE, TgTRUE );
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Forcefield( M_(VECTOR,DIM) tvFF )
{
    m_tvUFF = MATH::F_NORM( &m_tyFF, tvFF );
    m_tvFF = tvFF;
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Draw_Projectile( C_TgBOOL bFlag )
{
    m_bfFlags.Set_Flag( ETgFLAG_DRAW_PROJECTILE, bFlag );
};


template <typename TYPE, int DIM>
TgBOOL TTgCOL_DEBUG<TYPE,DIM>::Query_Draw_Projectile()
{
    return (m_bfFlags.Query_Flag( ETgFLAG_DRAW_PROJECTILE ));
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Push_Contact( CR_(CONTACT,DIM) tgContact )
{
    if (m_niContact < KTgMAX_CONTACT)
    {
        m_atgContact[m_niContact].m_tvPos = MATH::F_TX( m_tmRF, tgContact.m_tvPos );
        m_atgContact[m_niContact].m_tvNormal = MATH::F_TX( m_tmRF, tgContact.m_tvNormal );
        m_atgContact[m_niContact].m_tyT0 = tgContact.m_tyT0;
        m_atgContact[m_niContact].m_tyDepth = tgContact.m_tyDepth;

        ++m_niContact;
    };
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Push_Tri( CR_(PTRI,DIM) tgPT )
{
    if (m_niTri < KTgMAX_TRI)
    {
        register T_(VECTOR,DIM)             tvP0, tvP1, tvP2, tvNM;
        
        tvP0 = MATH::F_TX( m_tmRF, tgPT.Query_Point0() );
        tvP1 = MATH::F_TX( m_tmRF, tgPT.Query_Point1() );
        tvP2 = MATH::F_TX( m_tmRF, tgPT.Query_Point2() );

        for (TgINT iIdx = 0; iIdx < m_niTri; ++iIdx)
        {
            if (
                MATH::F_LSQ( MATH::F_SUB( tvP0, m_atgPT[iIdx].Query_Point0() ) ) >= LIMITS<TYPE>::ROOTEPSILON ||
                MATH::F_LSQ( MATH::F_SUB( tvP1, m_atgPT[iIdx].Query_Point1() ) ) >= LIMITS<TYPE>::ROOTEPSILON ||
                MATH::F_LSQ( MATH::F_SUB( tvP2, m_atgPT[iIdx].Query_Point2() ) ) >= LIMITS<TYPE>::ROOTEPSILON
            ) {
                continue;
            };

            return;
        };

        tvNM = MATH::F_TX( m_tmRF, tgPT.Query_Normal() );

        m_atgPT[m_niTri].Set_PointDN( tvP0,tvP1,tvP2, tvNM );
        m_aiID[m_niTri] = 0;

        ++m_niTri;
    };
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Tri_State( C_TgINT iState )
{
    if (m_niTri > 0)
    {
        m_aiID[m_niTri-1] = iState;
    };
};


template <typename TYPE, int DIM>
TgVOID TTgCOL_DEBUG<TYPE,DIM>::Set_Mask( C_TgUINT32 uiMask )
{
    m_uiTriMask = uiMask;
};


template <typename TYPE, int DIM>
TgUINT32 TTgCOL_DEBUG<TYPE,DIM>::Query_Mask()
{
    return (m_uiTriMask);
};


// ============================================================================================================================== //

#if defined(TgS_INSTANTIATE_TEMPLATES)

    template class TTgCOL_DEBUG<TgFLOAT32,4>;

#endif // TgS_INSTANTIATE_TEMPLATES

}; // END COL //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////